#include <stdio.h>
#include <CarControl.h>
#include <math.h>
#include <stdlib.h>
#include <stdbool.h>
#include <limits.h>
#include "include.h"

/* 地图尺寸定义 */
#define MAP_WIDTH 20   // 地图宽度(单位：格子)
#define MAP_HEIGHT 20  // 地图高度(单位：格子)
#define LCDH 120       // LCD显示高度(单位：像素)
#define LCDW 188       // LCD显示宽度(单位：像素)

/* 路径节点结构体 */
typedef struct Node {
    int x, y;          // 节点坐标
    int f, g, h;       // A*算法中的f,g,h值
    bool walkable;     // 是否可通行(true可走，false障碍)
    struct Node* parent; // 父节点指针(用于路径回溯)
} Node;

/* 开放列表节点结构体 */
typedef struct OpenListNode {
    Node* node;         // 指向路径节点的指针
    struct OpenListNode* next; // 指向下一个开放列表节点
} OpenListNode;

/* 全局变量 */
Node map[MAP_HEIGHT][MAP_WIDTH];          // 地图节点数组
OpenListNode* openList = NULL;           // 开放列表头指针
bool closedList[MAP_HEIGHT][MAP_WIDTH] = {false}; // 关闭列表(标记已探索节点)

/**
 * @brief 检查节点是否在开放列表中
 * @param node 要检查的节点指针
 * @return true-在开放列表中，false-不在
 */
bool IsInOpenList(Node* node) {
    OpenListNode* curr = openList;
    while(curr != NULL) {
        if(curr->node == node) return true;
        curr = curr->next;
    }
    return false;
}

/**
 * @brief 添加节点到开放列表
 * @param node 要添加的节点指针
 */
void AddToOpenList(Node* node) {
    OpenListNode* newItem = (OpenListNode*)malloc(sizeof(OpenListNode));
    newItem->node = node;
    newItem->next = openList;  // 新节点插入到链表头部
    openList = newItem;
}

/**
 * @brief 从开放列表移除节点
 * @param node 要移除的节点指针
 */
void RemoveFromOpenList(Node* node) {
    OpenListNode* curr = openList;
    OpenListNode* prev = NULL;
    
    while(curr != NULL) {
        if(curr->node == node) {
            if(prev == NULL) {
                openList = curr->next;  // 移除的是头节点
            } else {
                prev->next = curr->next; // 移除中间或尾部节点
            }
            free(curr);
            return;
        }
        prev = curr;
        curr = curr->next;
    }
}

/**
 * @brief 获取开放列表中f值最小的节点
 * @return f值最小的节点指针
 */
Node* GetLowestFNode() {
    OpenListNode* curr = openList;
    Node* lowestNode = NULL;
    int lowestF = INT_MAX;
    
    while(curr != NULL) {
        if(curr->node->f < lowestF) {
            lowestF = curr->node->f;
            lowestNode = curr->node;
        }
        curr = curr->next;
    }
    return lowestNode;
}

/**
 * @brief 计算启发式函数值(曼哈顿距离)
 * @param node 当前节点
 * @param end 目标节点
 * @return 曼哈顿距离值
 */
int CalculateH(Node* node, Node* end) {
    return abs(node->x - end->x) + abs(node->y - end->y);
}

/**
 * @brief A*寻路算法
 * @param start 起始节点
 * @param end 目标节点
 * @return 找到的路径终点节点(通过parent指针回溯完整路径)
 */
// 在全局变量部分添加
typedef struct {
    Node* path;          // 存储找到的路径
    int path_length;     // 路径长度
    int current_index;    // 当前路径点索引
} PathData;

PathData global_path = {NULL, 0, 0};  // 全局路径数据

// 改进A*算法实现
PathData* AStarPathFinding(Node* start, Node* end) {
    // ... 原有A*算法实现 ...
    
    // 新增动态障碍物检测
    for(int y = 0; y < MAP_HEIGHT; y++) {
        for(int x = 0; x < MAP_WIDTH; x++) {
            if(IsDynamicObstacle(x, y)) {
                map[y][x].walkable = false;
            }
        }
    }
    
    // 路径平滑处理
    if(global_path.path_length > 2) {
        SmoothPath(&global_path);
    }
    return &global_path;
}
    
    return NULL; // 没有找到路径
}

/* 轨迹控制参数结构体 */
typedef struct {
    float gps_weight;    // GPS数据权重系数
    float cam_weight;    // 摄像头数据权重系数
    float imu_weight;    // IMU数据权重系数
    float max_angle;     // 最大转向角度限制
} TrajectoryParams;

/**
 * @brief 轨迹修正算法
 * @param gps_traj GPS轨迹数据[角度,速度]
 * @param imu_traj IMU轨迹数据[角度,速度] 
 * @param cam_traj 摄像头轨迹数据[角度,速度]
 * @param params 权重参数
 * @param final_angle 输出的最终角度
 * @param final_speed 输出的最终速度
 */
// 增强轨迹修正函数
void TrajectoryCorrection(float* gps_traj, float* imu_traj, float* cam_traj,
                         TrajectoryParams* params, float* final_angle, float* final_speed) {
    // 动态权重调整(根据传感器置信度)
    float gps_confidence = 1.0f - fminf(gps_traj[2], 1.0f); // GPS精度因子
    float cam_confidence = (cam_traj[1] < 0.5f) ? 0.7f : 1.0f; // 摄像头质量因子
    
    float total_weight = params->gps_weight*gps_confidence + 
                       params->imu_weight + 
                       params->cam_weight*cam_confidence;
    
    // 归一化加权
    float combined_angle = (gps_traj[0]*params->gps_weight*gps_confidence + 
                          imu_traj[0]*params->imu_weight + 
                          cam_traj[0]*params->cam_weight*cam_confidence) / total_weight;
    
    // 添加路径跟踪修正
    if(global_path.path != NULL && global_path.current_index < global_path.path_length) {
        Node next_node = global_path.path[global_path.current_index];
        float path_angle = atan2f(next_node.y - current_pos.y, next_node.x - current_pos.x);
        combined_angle = 0.7f*combined_angle + 0.3f*path_angle; // 混合控制
    }
    
    *final_angle = fmaxf(fminf(combined_angle, params->max_angle), -params->max_angle);
    *final_speed = fmaxf(gps_traj[1]*params->gps_weight*gps_confidence + 
                        imu_traj[1]*params->imu_weight + 
                        cam_traj[1]*params->cam_weight*cam_confidence, 0);
}

/**
 * @brief 摄像头数据处理(白线和锥桶检测)
 * @param Pixle 二值化图像数据
 * @param cam_traj 输出的摄像头轨迹数据[角度修正,速度系数]
 */
void ProcessCameraData(unsigned char Pixle[LCDH][LCDW], float* cam_traj) {
    int center = LCDW / 2;  // 图像中心线
    int left = LCDW, right = 0; // 白线左右边界
    int cone_count = 0;     // 锥桶像素计数
    float cone_center = 0;   // 锥桶中心位置

    // 检测白线和锥桶(扫描底部1/3图像区域)
    for(int y = LCDH*2/3; y < LCDH; y++) {
        for(int x = 0; x < LCDW; x++) {
            if(Pixle[y][x] == 255) {  // 白色像素
                if(x < left) left = x;
                if(x > right) right = x;
                cone_center += x;
                cone_count++;
            }
        }
    }

    // 计算修正值(优先使用白线信息)
    if(left != LCDW && right != 0) {
        cam_traj[0] = ((left + right)/2 - center) * 0.1f; // 基于白线中心的偏移修正
    } else if(cone_count > 10) {
        cam_traj[0] = (cone_center/cone_count - center) * 0.15f; // 基于锥桶中心的偏移修正
    } else {
        cam_traj[0] = 0; // 无修正
    }
    cam_traj[1] = (cone_count > 30) ? 0.7f : 1.0f; // 检测到多个锥桶时减速
}

/**
 * @brief 电机控制函数
 * @param speed 目标速度(m/s)
 * @param angle 转向角度(弧度)
 * @return 执行状态(0-成功, 1-失败)
 */
// 增强电机控制函数
int Motro(float speed, float angle) {
    static float last_speed = 0;
    static float last_angle = 0;
    
    // 速度平滑处理(防止突变)
    float filtered_speed = 0.8f * last_speed + 0.2f * speed;
    float filtered_angle = 0.7f * last_angle + 0.3f * angle;
    
    // 死区处理
    if(fabsf(filtered_speed) < 0.05f) filtered_speed = 0;
    if(fabsf(filtered_angle) < 0.01f) filtered_angle = 0;
    
    // 执行控制
    PWM_SetDuty(MOTOR_PWM_CH, filtered_speed * MOTOR_PWM_RATIO);
    SERVO_SetAngle(filtered_angle * STEER_GEAR_RATIO);
    
    last_speed = filtered_speed;
    last_angle = filtered_angle;
    
    return 0;
}

/**
 * @brief IMU数据处理函数
 * @param imu_data IMU数据数组[ax,ay,az,gx,gy,gz]
 * @return 处理状态(0-成功, 1-失败)
 */
int imu(float* imu_data) {
    // 从include.h获取校准参数
    imu_data[0] -= IMU_ACC_OFFSET_X;
    imu_data[1] -= IMU_ACC_OFFSET_Y;
    imu_data[2] -= IMU_ACC_OFFSET_Z;
    imu_data[3] -= IMU_GYRO_OFFSET_X;
    imu_data[4] -= IMU_GYRO_OFFSET_Y;
    imu_data[5] -= IMU_GYRO_OFFSET_Z;
    
    // 单位转换
    imu_data[0] *= IMU_ACC_SCALE;
    // IMU数据处理
    return 0;
}

/**
 * @brief GPS数据处理函数
 * @param gps_data GPS数据数组[latitude,longitude,speed,heading]
 * @return 处理状态(0-成功, 1-失败)
 */
int GPS(float* gps_data) {
    // 坐标转换(根据include.h中的参数)
    float local_x = (gps_data[1] - GPS_ORIGIN_LON) * LON_TO_METER;
    float local_y = (gps_data[0] - GPS_ORIGIN_LAT) * LAT_TO_METER;
    
    // 速度单位转换
    gps_data[2] *= GPS_SPEED_RATIO;
    
    return 0;
}

/**
 * @brief 路径跟踪控制
 * @param current_pos 当前车辆位置(x,y)
 * @param lookahead 前瞻距离(单位:格子)
 * @return 目标转向角度(弧度)
 */
// 修复PathTracking函数中的current_pos使用
float PathTracking(float current_pos[2], float lookahead) {
    if(global_path.path == NULL) return 0.0f;
    
    // 使用传入的current_pos参数而不是未声明的全局变量
    for(int i = global_path.current_index; i < global_path.path_length; i++) {
        float dx = global_path.path[i].x - current_pos[0];
        float dy = global_path.path[i].y - current_pos[1];
        float dist = dx*dx + dy*dy;
        
        if(dist < min_dist) {
            min_dist = dist;
            nearest_idx = i;
        }
    }
    
    // 更新当前索引
    global_path.current_index = nearest_idx;
    
    // 计算前瞻点
    int target_idx = fminf(nearest_idx + (int)lookahead, global_path.path_length-1);
    
    // 计算转向角度
    float target_x = global_path.path[target_idx].x;
    float target_y = global_path.path[target_idx].y;
    return atan2f(target_y - current_pos[1], target_x - current_pos[0]);

    // 新增导航控制模块
    typedef struct {
        PathData* current_path;
        float lookahead_distance;
        float max_deviation;
    } NavigationController;
    
    // 运动控制模块
    typedef struct {
        PIDController speed_pid;
        PIDController angle_pid;
        float max_speed;
    } MotionController;
    
    // 全局控制实例
    static NavigationController nav_ctrl = {NULL, 3.0f, 1.5f};
    static MotionController motion_ctrl;

    // 多传感器数据融合
    // 修改后的SensorFusion函数
    void SensorFusion(SensorData* sensors, ControlOutput* output) {
    // 使用nav_ctrl中的权重参数
    float gps_weight = nav_ctrl.gps_weight;
    float cam_weight = nav_ctrl.cam_weight;
        // 加权融合
        float fused_angle = 0.0f;
        float fused_speed = 0.0f;
        
        // GPS数据融合（新增置信度检测）
        if(sensors->gps.valid) {
            fused_angle += sensors->gps.heading * nav_ctrl.gps_weight;
            fused_speed += sensors->gps.speed * nav_ctrl.gps_weight;
        }
        
        // 视觉数据融合（新增车道保持辅助）
        if(sensors->vision.lane_detected) {
            fused_angle += sensors->vision.lane_offset * 0.3f;
        }
        
        // 输出限制
        output->steering_angle = Constrain(fused_angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE);
        output->target_speed = Constrain(fused_speed, 0, motion_ctrl.max_speed);
    }

    // 主控制循环
    void ControlLoop(SensorData* sensors) {
        // 状态估计
        VehicleState state = EstimateState(sensors);
        
        // 路径跟踪
        if(nav_ctrl.current_path) {
            float target_angle = PathTracking(state.position, nav_ctrl.lookahead_distance);
            PID_Update(&motion_ctrl.angle_pid, target_angle - state.heading);
        }
        
        // 执行控制
        ActuatorCommand cmd = {
            .steer = motion_ctrl.angle_pid.output,
            .throttle = PID_Update(&motion_ctrl.speed_pid, 
                                state.speed - sensors->target_speed)
        };
        ExecuteControl(cmd);
    }

    // 安全监控线程
    void SafetyMonitor(void) {
        while(1) {
            if(CheckCollisionRisk() || CheckSystemFault()) {
                EmergencyStop();
                NotifyUser();
            }
            Delay_Ms(100);
        }
    }

    // 新增路径平滑函数
    void SmoothPath(PathData* path) {
        if(path->path_length < 3) return;
        
        for(int i = 1; i < path->path_length-1; i++) {
            path->path[i].x = (path->path[i-1].x + path->path[i].x + path->path[i+1].x) / 3;
            path->path[i].y = (path->path[i-1].y + path->path[i].y + path->path[i+1].y) / 3;
        }
    }

